Constrained Initialization of the Simultaneous Localization and Mapping Algorithm

In this paper we present a novel feature initialization technique for the Simultaneous Localization and Mapping (SLAM) algorithm. The initialization scheme extends previous approaches for identifying new confirmed features and is shown to improve the steady-state performance of the filter by incorporating tentative features into the filter as soon as they are observed. Constraints are then applied between multiple feature estimates when a feature is confirmed. Observations that are subsequently deemed as spurious are removed from the state vector after an appropriate timeout. It is shown that information that would otherwise be lost can therefore be used consistently in the filter. Results of this algorithm applied to data collected using a submersible vehicle are also shown. KEY WORDS—simultaneous localization and mapping, feature initialization, constraint, submersible